#include "cservowindow.h"

#include <buola/gui/ctoolkit.h>
#include <buola/gui/cicon.h>
#include <buola/gui/cmenu.h>
#include <buola/gui/csimpleapp.h>
#include <buola/io/cfolder.h>
#include <buola/io/cresourcefinder.h>
#include <buola/io/cipendpoint.h>
#include <buola/io/csocket.h>
#include <buola/image/algorithm.h>
#include <buola/image/io.h>
#include <buola/app/ceventloop.h>
#include <buola/mat/transformations.h>
#include <buola/cv/coccupancyoctree.h>
#include <buola/mat/cgaussianprocess.h>
#include <buola/mat/cmatfile.h>
#include <buola/app/ccmdline.h>
#include <buola/algorithm/comparison.h>
#include <buola/scene/controllers/ccameracontroller.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/widgets/ctoolbar.h>
#include <buola/threads/thread.h>
#include <buola/scene/transform/crttransform.h>
#include <buola/scene/transform/cprismaticjoint.h>
#include <buola/scene/transform/cmattransform.h>
#include <buola/image/cgc.h>
#include <buola/scene/geodes/cplane.h>
#include <buola/scene/geodes/ccube.h>
#include <buola/zz/operators.h>
#include <buola/zz/zinteger.h>
#include <buola/zz/zraw.h>
#include <buola/zz/zchar.h>
#include <buola/zz/zreal.h>
#include <buola/zz/zliteral.h>

namespace buola { namespace simulator {

CServoWindow::CServoWindow(const io::CURI& pURDF)
{
    mAcceptor=io::listen_at(io::CIPEndPoint(22333));
    mAcceptor->AcceptAsync(std::bind(&CServoWindow::OnNewConnection,this,_1));
    
    SetupWindow();
    SetupScene(pURDF);
    SetupViews();
    SetupInput();
    AddCommitHook([this](const CRegion &pRegion){OnCommitHook(pRegion);});
    NextIteration(false,1);
}

CServoWindow::CServoWindow(const io::CURI& pURDF, const io::CURI& pPCD,const io::CURI &pJoints)
{
    SetupWindow();
    SetupScene(pURDF,pJoints);
    SetupViews();
    SetupInput(pPCD);
    AddCommitHook([this](const CRegion &pRegion){OnCommitHook(pRegion);});
    NextIteration(false,1);
}

CServoWindow::CServoWindow(const io::CURI& pURDF, const io::CURI& pRGB,const io::CURI &pDepth,const io::CURI &pJoints)
{
    SetupWindow();
    SetupScene(pURDF,pJoints);
    SetupViews();
    SetupInput(pRGB,pDepth);
    AddCommitHook([this](const CRegion &pRegion){OnCommitHook(pRegion);});
    NextIteration(false,1);
}

CServoWindow::~CServoWindow()
{
}

void CServoWindow::SetupWindow()
{
    SetNumRows(3);
    mToolBar=new gui::CToolBar();
    Attach(mToolBar,gui::ATTACH_NORTH,gui::ATTACH_TOOLBAR);

    gui::PMenu lMenu=new gui::CMenu;
    
    lMenu->Add(gui::new_menu_item(L"next iteration",ICON_STOCK_ARROW_RIGHT))->
              eTriggered.Connect(&CServoWindow::NextIteration,this,true,1);
    lMenu->Add(gui::new_menu_item(L"25 iterations",ICON_STOCK_ARROW_RIGHT_DOUBLE))->
              eTriggered.Connect(&CServoWindow::NextIteration,this,true,25);
    
    mToolBar->SetMenu(lMenu);
}

void CServoWindow::SetupScene(const io::CURI &pURDF,const io::CURI &pJoints)
{
    mScene=new scene::CScene;
    scene::PRTTransform lBefore=new scene::CRTTransform;
    scene::PRTTransform lVVSTransform=new scene::CRTTransform;
    scene::PRTTransform lAfter=new scene::CRTTransform;
    lVVSTransform->SetTranslation(mat::CVec3d(0,0,0));
    lVVSTransform->SetRotation(mat::CQuat_d());
    mScene->World()->AddChild(lAfter);
    lAfter->AddChild(lVVSTransform);
    lVVSTransform->AddChild(lBefore);

    scene::PTransform lObject=scene::CTransform::Import(pURDF);
    lBefore->AddChild(lObject);

    SetupJoints(pJoints);
    mat::CMat34d lHandTrans=lObject->FindDescendant("r_elbow_flex_joint")->GetWorldTransform();
    mat::CVec3d lHandPos=lHandTrans*mat::CVec3d(0,0,0);
    msg_info() << lHandPos << "\n";
    lBefore->SetTranslation(-lHandPos);
    lAfter->SetTranslation(lHandPos);
    mScene->Update();
    mOpticalFrame=lObject->FindDescendant("head_mount_kinect_rgb_optical_frame");
    mKinectTrans=mOpticalFrame->GetWorldTransform();

    mVVSCamera=new scene::CPerspectiveCamera;
    mVVSCamera->SetClipping(0.100,10);
    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    mVVSCamera->SetShift(lParams.GetC()-CSize_d(320,240),CSize_d(640,480));
    mVVSCamera->SetFOV(2*atan2(240,lParams.GetF().y));

    mVVSCamera->LookFrom(mKinectTrans*mat::CVec3d(0,0,0),mat::CQuat_d(mKinectTrans)*mat::CQuat_d(90_deg,mat::CVec3d(1,0,0)));
    
    mVVS.SetScene(mScene,lVVSTransform);
    mVVS.SetCamera(mVVSCamera);
    
    lObject->TraverseTree(
        [](const scene::PTransform &pT)
        {
            if(!starts_with(pT->Name(),"r_")&&!starts_with(pT->Name(),"l_"))
                pT->RemoveAllObjects();
        }
    );
}

void CServoWindow::SetupScene(const io::CURI &pURDF)
{
    mScene=new scene::CScene;
    scene::PRTTransform lBefore=new scene::CRTTransform;
    scene::PRTTransform lVVSTransform=new scene::CRTTransform;
    scene::PRTTransform lAfter=new scene::CRTTransform;
    lVVSTransform->SetTranslation(mat::CVec3d(0,0,0));
    lVVSTransform->SetRotation(mat::CQuat_d());
    mScene->World()->AddChild(lAfter);
    lAfter->AddChild(lVVSTransform);
    lVVSTransform->AddChild(lBefore);

    scene::PTransform lObject=scene::CTransform::Import(pURDF);
    lBefore->AddChild(lObject);

    mat::CMat34d lHandTrans=lObject->FindDescendant("r_elbow_flex_joint")->GetWorldTransform();
    mat::CVec3d lHandPos=lHandTrans*mat::CVec3d(0,0,0);
    msg_info() << lHandPos << "\n";
    lBefore->SetTranslation(-lHandPos);
    lAfter->SetTranslation(lHandPos);
    mScene->Update();
    mOpticalFrame=lObject->FindDescendant("head_mount_kinect_rgb_optical_frame");
    mKinectTrans=mOpticalFrame->GetWorldTransform();

    mVVSCamera=new scene::CPerspectiveCamera;
    mVVSCamera->SetClipping(0.100,10);
    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    mVVSCamera->SetShift(lParams.GetC()-CSize_d(320,240),CSize_d(640,480));
    mVVSCamera->SetFOV(2*atan2(240,lParams.GetF().y));

    mVVSCamera->LookFrom(mKinectTrans*mat::CVec3d(0,0,0),mat::CQuat_d(mKinectTrans)*mat::CQuat_d(90_deg,mat::CVec3d(1,0,0)));
    
    mVVS.SetScene(mScene,lVVSTransform);
    mVVS.SetCamera(mVVSCamera);
    
    lObject->TraverseTree(
        [](const scene::PTransform &pT)
        {
            if(!starts_with(pT->Name(),"r_")&&!starts_with(pT->Name(),"l_"))
                pT->RemoveAllObjects();
        }
    );
}

void set_joint(scene::PTransform pWhere,const std::string &pName,double pValue)
{
    auto lRev=scene::find_revolution_joint(pWhere,pName);
    if(lRev)
    {
        lRev->SetValue(pValue);
        return;
    }
    auto lPri=scene::find_prismatic_joint(pWhere,pName);
    if(lPri)
    {
        lPri->SetValue(pValue);
        return;
    }
    
    msg_warn() << "can't find joint!!!!\n";
}

void CServoWindow::SetupJoints(const io::CURI &pJoints)
{
    scene::PTransform lObject=mScene->World();

    std::map<int,std::string> lJointNames;
    std::map<int,double> lJointValues;
    
    io::CTextReader lS(pJoints);
    std::string lStr;
    while(get_line(lS,lStr))
    {
        if(starts_with(lStr,"  name"))
        {
            typedef std::string::const_iterator TIt;
            std::tuple<int,std::pair<TIt,TIt>> lAttr;
            if(!zz::parse(lStr.begin(),lStr.end(),"  name["_z >> zz::int32() >> "]: "_z >> zz::raw(+zz::chr()),lAttr)) continue;
            msg_info() << std::get<0>(lAttr) << " " << std::string(std::get<1>(lAttr).first,std::get<1>(lAttr).second) << "\n";
            lJointNames[std::get<0>(lAttr)]=std::string(std::get<1>(lAttr).first,std::get<1>(lAttr).second);
        }
        if(starts_with(lStr,"  position"))
        {
            typedef std::string::const_iterator TIt;
            std::tuple<int,double> lAttr;
            if(!zz::parse(lStr.begin(),lStr.end(),"  position["_z >> zz::int32()>> "]: "_z >> zz::dbl(),lAttr)) continue;
            msg_info() << std::get<0>(lAttr) << " " << std::get<1>(lAttr) << "\n";
            lJointValues[std::get<0>(lAttr)]=std::get<1>(lAttr);
        }
    }
    
    for(auto n : lJointNames)
    {
        auto v=lJointValues.find(n.first);
        if(v==lJointValues.end())
        {
            msg_warn() << "joint not found!\n";
            continue;
        }
        set_joint(lObject,n.second,v->second);
    }
    
    mScene->Update();
}

void CServoWindow::SetupJoints(const std::array<double,45> &pJoints)
{
    scene::PTransform lObject=mScene->World();

    set_joint(lObject,"fl_caster_rotation_joint",pJoints[0]);
    set_joint(lObject,"fl_caster_l_wheel_joint",pJoints[1]);
    set_joint(lObject,"fl_caster_r_wheel_joint",pJoints[2]);
    set_joint(lObject,"fr_caster_rotation_joint",pJoints[3]);
    set_joint(lObject,"fr_caster_l_wheel_joint",pJoints[4]);
    set_joint(lObject,"fr_caster_r_wheel_joint",pJoints[5]);
    set_joint(lObject,"bl_caster_rotation_joint",pJoints[6]);
    set_joint(lObject,"bl_caster_l_wheel_joint",pJoints[7]);
    set_joint(lObject,"bl_caster_r_wheel_joint",pJoints[8]);
    set_joint(lObject,"br_caster_rotation_joint",pJoints[9]);
    set_joint(lObject,"br_caster_l_wheel_joint",pJoints[10]);
    set_joint(lObject,"br_caster_r_wheel_joint",pJoints[11]);
    set_joint(lObject,"torso_lift_joint",pJoints[12]);
    set_joint(lObject,"torso_lift_motor_screw_joint",pJoints[13]);
    set_joint(lObject,"head_pan_joint",pJoints[14]);
    set_joint(lObject,"head_tilt_joint",pJoints[15]);
    set_joint(lObject,"laser_tilt_mount_joint",pJoints[16]);
    set_joint(lObject,"r_upper_arm_roll_joint",pJoints[17]);
    set_joint(lObject,"r_shoulder_pan_joint",pJoints[18]);
    set_joint(lObject,"r_shoulder_lift_joint",pJoints[19]);
    set_joint(lObject,"r_forearm_roll_joint",pJoints[20]);
    set_joint(lObject,"r_elbow_flex_joint",pJoints[21]);
    set_joint(lObject,"r_wrist_flex_joint",pJoints[22]);
    set_joint(lObject,"r_wrist_roll_joint",pJoints[23]);
    set_joint(lObject,"r_gripper_joint",pJoints[24]);
    set_joint(lObject,"r_gripper_l_finger_joint",pJoints[25]);
    set_joint(lObject,"r_gripper_r_finger_joint",pJoints[26]);
    set_joint(lObject,"r_gripper_r_finger_tip_joint",pJoints[27]);
    set_joint(lObject,"r_gripper_l_finger_tip_joint",pJoints[28]);
    set_joint(lObject,"r_gripper_motor_screw_joint",pJoints[29]);
    set_joint(lObject,"r_gripper_motor_slider_joint",pJoints[30]);
    set_joint(lObject,"l_upper_arm_roll_joint",pJoints[31]);
    set_joint(lObject,"l_shoulder_pan_joint",pJoints[32]);
    set_joint(lObject,"l_shoulder_lift_joint",pJoints[33]);
    set_joint(lObject,"l_forearm_roll_joint",pJoints[34]);
    set_joint(lObject,"l_elbow_flex_joint",pJoints[35]);
    set_joint(lObject,"l_wrist_flex_joint",pJoints[36]);
    set_joint(lObject,"l_wrist_roll_joint",pJoints[37]);
    set_joint(lObject,"l_gripper_joint",pJoints[38]);
    set_joint(lObject,"l_gripper_l_finger_joint",pJoints[39]);
    set_joint(lObject,"l_gripper_r_finger_joint",pJoints[40]);
    set_joint(lObject,"l_gripper_r_finger_tip_joint",pJoints[41]);
    set_joint(lObject,"l_gripper_l_finger_tip_joint",pJoints[42]);
    set_joint(lObject,"l_gripper_motor_screw_joint",pJoints[43]);
    set_joint(lObject,"l_gripper_motor_slider_joint",pJoints[44]);

    mScene->Update();
}

void CServoWindow::SetupViews()
{
    mSceneView=new scene::CSceneView;
    
    mSceneView->AddScene(mScene);
    mCloudScene=new scene::CScene;
    mSceneCloud=new scene::CPointCloud;
    mCloudTransform=new scene::CMatTransform(mKinectTrans);
    mCloudScene->World()->AddChild(mCloudTransform);
    mCloudTransform->AddObject(mSceneCloud);
    mSceneView->AddScene(mCloudScene);
    mSceneCamera=new scene::CPerspectiveCamera;
    mSceneCamera->SetFOV(2*atan2(240.0,525.0));
    mSceneCamera->SetClipping(0.10,50);
    msg_info() << mKinectTrans << "\n";
    mSceneCamera->LookFrom(mKinectTrans*mat::CVec3d(0,0,0),mat::CQuat_d(mKinectTrans)*mat::CQuat_d(90_deg,mat::CVec3d(1,0,0)));
    mSceneView->SetCamera(mSceneCamera);
    mSceneView->AddController(new scene::CCameraController(mSceneCamera));
    mSceneView->Resize({640,480});
    mSceneView->SetTitle(L"current iteration");
    
    Add(mSceneView);
    
    mObjectView=new scene::CSceneView;
    
    mObjectScene=new scene::CScene;
    mObjectCloud=new scene::CPointCloud;
    mObjectTransform=new scene::CMatTransform(mKinectTrans);
    mObjectScene->World()->AddChild(mObjectTransform);
    mObjectTransform->AddObject(mObjectCloud);
    mObjectView->AddScene(mObjectScene);
    mObjectCamera=new scene::CPerspectiveCamera;
    mObjectCamera->SetFOV(2*atan2(240.0,525.0));
    mObjectCamera->SetClipping(0.10,50);
    mObjectCamera->LookFrom(mKinectTrans*mat::CVec3d(0,0,0),mat::CQuat_d(mKinectTrans)*mat::CQuat_d(90_deg,mat::CVec3d(1,0,0)));
    mObjectView->SetCamera(mObjectCamera);
    mObjectView->AddController(new scene::CCameraController(mObjectCamera));
    mObjectView->Resize({640,480});
    mObjectView->SetTitle(L"object");
    
    Add(mObjectView);
    
    mNumbersBox=new gui::CListContainer;
    mNumbersBox->AddGroup(L"pose",0);
    mNumbersBox->AddGroup(L"method",1);
    mNumbersBox->Resize({200,480});
    for(int i=0;i<6;i++)
    {
        mCoordsBox[i]=new gui::CNumBox;
        mCoordsBox[i]->Resize({200,20});
        mNumbersBox->Add(mCoordsBox[i],0);
        if(i<3)
            mCoordsBox[i]->SetStep(0.02);
        else
            mCoordsBox[i]->SetStep(1);
        mCoordsBox[i]->SetStyle(gui::CNumBox::STYLE_DOUBLE);
        mCoordsBox[i]->eNumChange.Connect(&CServoWindow::OnCoordsBox,this,i);
    }
    
    mCoordsBox[0]->SetTitle(L"x");
    mCoordsBox[1]->SetTitle(L"y");
    mCoordsBox[2]->SetTitle(L"z");
    mCoordsBox[3]->SetTitle(L"a");
    mCoordsBox[4]->SetTitle(L"b");
    mCoordsBox[5]->SetTitle(L"c");
    
    m2DBox=new gui::CCheckBox;
    m2DBox->Resize({200,20});
    m2DBox->Check();
    m2DBox->SetTitle(L"2D");
    mNumbersBox->Add(m2DBox,1);
    
    m3DBox=new gui::CCheckBox;
    m3DBox->Resize({200,20});
    m3DBox->Check();
    m3DBox->SetTitle(L"3D");
    mNumbersBox->Add(m3DBox,1);
    
    Add(mNumbersBox);

    mat::CVec3d lPosition=mVVS.Position();
    mat::C3DRotation lRotation(mVVS.Rotation());
    
    mCoordsBox[0]->SetValue(lPosition.x());
    mCoordsBox[1]->SetValue(lPosition.y());
    mCoordsBox[2]->SetValue(lPosition.z());
    mCoordsBox[3]->SetValue(rad2deg(lRotation.a));
    mCoordsBox[4]->SetValue(rad2deg(lRotation.b));
    mCoordsBox[5]->SetValue(rad2deg(lRotation.c));
}

void CServoWindow::SetupInput(const io::CURI &pPCD)
{
    img::load_rgbd(mTargetImage,mTargetDepth,pPCD);

    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    mTargetXYZ=cvi::make_pos_image(mTargetDepth,lParams);
    mSceneCloud->SetCloud(cvi::make_cloud(mTargetDepth,mTargetImage,lParams));
    
    mVVS.SetTarget(mTargetImage,mTargetDepth);
    mVVS.SetInitialPose(mat::CVec3d(0,0,0),mat::CQuat_d());
}

void CServoWindow::SetupInput(const io::CURI &pRGB,const io::CURI &pDepth)
{
    img::load(mTargetImage,pRGB);
    img::load_ascii(mTargetDepth,pDepth,mTargetImage.Size());
    
    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    
    mTargetXYZ=cvi::make_pos_image(mTargetDepth,lParams);
    mSceneCloud->SetCloud(cvi::make_cloud(mTargetDepth,mTargetImage,lParams));
    
    mVVS.SetTarget(mTargetImage,mTargetDepth);
    mVVS.SetInitialPose(mat::CVec3d(0,0,0),mat::CQuat_d());
}

void CServoWindow::SetupInput(const float *pPos,const uint8_t *pRGB)
{
    mTargetDepth.Recreate({640,480});
    mTargetImage.Recreate({640,480});
    
    for(int i=0;i<640;i++)
    {
        for(int j=0;j<480;j++)
        {
            mTargetImage(i,j)[0]=pRGB[3*(640*j+i)];
            mTargetImage(i,j)[1]=pRGB[3*(640*j+i)+1];
            mTargetImage(i,j)[2]=pRGB[3*(640*j+i)+2];
            mTargetDepth(i,j)=pPos[3*(640*j+i)+2];
        }
    }

    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    
    mTargetXYZ=cvi::make_pos_image(mTargetDepth,lParams);
    mSceneCloud->SetCloud(cvi::make_cloud(mTargetDepth,mTargetImage,lParams));
    
    mVVS.SetTarget(mTargetImage,mTargetDepth);
    mVVS.SetInitialPose(mat::CVec3d(0,0,0),mat::CQuat_d());
}

void CServoWindow::SetupInput()
{
    mTargetDepth.Recreate({640,480});
    mTargetImage.Recreate({640,480});
    
    for(int i=0;i<640;i++)
    {
        for(int j=0;j<480;j++)
        {
            mTargetImage(i,j)[0]=0;
            mTargetImage(i,j)[1]=0;
            mTargetImage(i,j)[2]=0;
            mTargetDepth(i,j)=1.0;
        }
    }

    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    
    mTargetXYZ=cvi::make_pos_image(mTargetDepth,lParams);
    mSceneCloud->SetCloud(cvi::make_cloud(mTargetXYZ,mTargetImage));
    
    mVVS.SetTarget(mTargetImage,mTargetDepth);
    mVVS.SetInitialPose(mat::CVec3d(0,0,0),mat::CQuat_d());
}

void CServoWindow::ShakeServo()
{
    mat::CVec3d lCurrentPos=mVVS.Position();
    mat::CQuat_d lCurrentRot=mVVS.Rotation();
    mVVS.SetInitialPose(lCurrentPos+mat::CVec3d(-0.030+0.060*random::get<double>(),-0.030+0.060*random::get<double>(),
                                                -0.030+0.060*random::get<double>()),lCurrentRot);
    Refresh();
}

void CServoWindow::ResetServo()
{
    mVVS.SetInitialPose(mat::CVec3d(0,0,0),mat::CQuat_d());
    Refresh();
}

void CServoWindow::NextIteration(bool pReally,int pCount)
{
    for(int i=0;i<pCount;i++)
        mVVS.Iterate(pReally,m2DBox->IsChecked(),m3DBox->IsChecked());
 
    mDirectionsImage.Recreate(mTargetImage.Size());
    mVVS.GetDirectionsImage(mDirectionsImage);

    auto lInputImage=clone(mTargetImage);
    mVVS.DrawRenderEdges(lInputImage);

    if(!mImageBox[0])
    {
        for(int i=0;i<3;i++)
        {
            mImageBox[i]=new gui::CImageBox;
            mImageBox[i]->SetTitle(L"something");
            Add(mImageBox[i]);
        }

        mImageBox[0]->SetTitle(L"input");
        mImageBox[1]->SetTitle(L"render");
        mImageBox[2]->SetTitle(L"edges");
    }

    mImageBox[0]->SetImage(std::move(lInputImage));
    mImageBox[1]->SetImage(clone(mVVS.LastRender()));
    mImageBox[2]->SetImage(clone(mDirectionsImage));
    mSceneView->Refresh();
    
    mat::CVec3d lPosition=mVVS.Position();
    mat::C3DRotation lRotation(mVVS.Rotation());
    
    mCoordsBox[0]->SetValue(lPosition.x());
    mCoordsBox[1]->SetValue(lPosition.y());
    mCoordsBox[2]->SetValue(lPosition.z());
    mCoordsBox[3]->SetValue(rad2deg(lRotation.a));
    mCoordsBox[4]->SetValue(rad2deg(lRotation.b));
    mCoordsBox[5]->SetValue(rad2deg(lRotation.c));
    
    mTransformIndex=clone(mVVS.TransformIndex());
    const auto &lLastDepth=mVVS.LastDepth();
    const mat::CMat4d &lProjection=mVVS.LastProjection();

    cvi::CCloud_rgb8 lObjectCloud;
    
    for(int x=0;x<640;x++)
    {
        for(int y=0;y<480;y++)
        {
            double lTargetDepth=mTargetDepth(x,y);
            if(is_nan(lTargetDepth)) continue;
            if(lTargetDepth>1) continue;
            
            if(lLastDepth(x,y)!=(uint32_t)-1)
            {
                double lFDepth=2.0*lLastDepth(x,y)/((double)(unsigned int)-1)-1.0; //from -1 to 1
                double lNewDepth=lProjection(2,3)/(lFDepth+lProjection(2,2));
                if(lTargetDepth>lNewDepth-0.03) continue;
            }
            
            mTransformIndex(x,y)=0;
            
            typename cvi::CCloud_rgb8::TPoint lPoint;
            lPoint.mPixel=mTargetImage(x,y);
            lPoint.mPoint=mat::CVec3f(mTargetXYZ(x,y)[0],mTargetXYZ(x,y)[1],mTargetXYZ(x,y)[2]); ///\todo fix this!
            lObjectCloud.push_back(lPoint);
        }
    }
    
    mObjectCloud->SetCloud(std::move(lObjectCloud));
    mObjectView->Refresh();
}

void CServoWindow::OnCoordsBox(int i)
{
    mat::CVec3d lPosition(mCoordsBox[0]->GetDoubleValue(),mCoordsBox[1]->GetDoubleValue(),mCoordsBox[2]->GetDoubleValue());
    mat::C3DRotation lRotation=deg2rad(mat::C3DRotation(mCoordsBox[3]->GetDoubleValue(),mCoordsBox[4]->GetDoubleValue(),mCoordsBox[5]->GetDoubleValue()));
    
    mVVS.SetInitialPose(lPosition,mat::CQuat_d(lRotation));
    NextIteration(false);
    
    mSceneView->Refresh();
}

void CServoWindow::OnCommitHook(const CRegion &pRegion)
{
    return;
    msg_info() << "oncommithook\n";
    CSize_i lSize((int)Size().x/4*4,(int)Size().y/4*4);
    img::CImage_rgb8 lImage(lSize);
     
    start_timer();
    glReadBuffer(GL_FRONT);
    glReadPixels(0,0,lSize.x,lSize.y,GL_RGB,GL_UNSIGNED_BYTE,lImage.RawData());
    glReadBuffer(GL_BACK);
    end_timer();
    
    img::save(lImage,io::file("commit.ppm"));
}

void CServoWindow::OnNewConnection(io::PSocket pSocket)
{
    msg_info() << "got new connection!!\n";

    std::vector<float> lPosBuf(640*480*3);
    int a=pSocket->Receive(&lPosBuf[0],640*480*3*sizeof(float),MSG_WAITALL);
    msg_info() << a << "\n";
    std::vector<uint8_t> lColorBuf(640*480*3);
    a=pSocket->Receive(&lColorBuf[0],640*480*3,MSG_WAITALL);
    msg_info() << a << "\n";
    std::array<double,45> lJointsBuf;
    a=pSocket->Receive(&lJointsBuf[0],45*sizeof(double),MSG_WAITALL);
    msg_info() << a << "\n";
    
    SetupJoints(lJointsBuf);
    SetupInput(&lPosBuf[0],&lColorBuf[0]);
    mScene->Update();
    
    mKinectTrans=mOpticalFrame->GetWorldTransform();

    mObjectTransform->SetMatrix(mKinectTrans);
    mObjectScene->Update();
    mCloudTransform->SetMatrix(mKinectTrans);
    mCloudScene->Update();
    
    mVVSCamera->LookFrom(mKinectTrans*mat::CVec3d(0,0,0),mat::CQuat_d(mKinectTrans)*mat::CQuat_d(90_deg,mat::CVec3d(1,0,0)));
    mSceneCamera->LookFrom(mKinectTrans*mat::CVec3d(0,0,0),mat::CQuat_d(mKinectTrans)*mat::CQuat_d(90_deg,mat::CVec3d(1,0,0)));
    mObjectCamera->LookFrom(mKinectTrans*mat::CVec3d(0,0,0),mat::CQuat_d(mKinectTrans)*mat::CQuat_d(90_deg,mat::CVec3d(1,0,0)));
    
    start_timer();
    NextIteration(true,10);
    end_timer();
    msg_info() << pSocket->Send(mTransformIndex.RawBytes(),640*480,MSG_WAITALL) << "\n";
}

/*namespace simulator*/ } /*namespace buola*/ }

int main(int pNArgs,char **pArgs)
{
    using namespace buola;
    
    buola_init(pNArgs,pArgs);

    try
    {
        gui::get_toolkit()->GetOpenGLGC()->MakeCurrent();
        io::CURI lFolder;
        if(cmd_line().ArgCount()==4)
        {
            gui::CSimpleApp<simulator::CServoWindow> lApp(io::arg(0),io::arg(1),io::arg(2),io::arg(3));
            lApp.Run();
        }
        else if(cmd_line().ArgCount()==3)
        {
            gui::CSimpleApp<simulator::CServoWindow> lApp(io::arg(0),io::arg(1),io::arg(2));
            lApp.Run();
        }
        else if(cmd_line().ArgCount()==1)
        {
            gui::CSimpleApp<simulator::CServoWindow> lApp(io::arg(0));
            lApp.Run();
        }
        else
            throw XArgument("wrong number of arguments... specify URDF, RGB, depth and joints files");
    }
    catch(std::exception &pE)
    {
        msg_error() << pE.what() << "\n";
    }

    return buola_finish();
}

